/**
  ******************************************************************************
  * File Name          : pid.c
  * Description        : pid计算相关
  ******************************************************************************
  */
#include "pid.h"


void PID_Calc(PID_Regulator_t *pid)
{
	float dError,Error;

	Error = pid->ref - pid->fdb;

	pid->sum += Error;
	if(pid->sum > pid->sumMax) pid->sum = pid->sumMax;
	if(pid->sum < -pid->sumMax) pid->sum = -pid->sumMax;
	pid->err[1] = pid->err[0];
	pid->err[0] = Error;
	dError = pid->err[0]-pid->err[1];

	pid->output = pid->kp*Error + pid->ki*pid->sum + pid->kd*dError;

	if(pid->output > pid->outputMax)
		pid->output = pid->outputMax;
	else if(pid->output < -pid->outputMax)
		pid->output = -pid->outputMax;
	
}
